#include "Mechanum.h"


void Mechanum::Update(float _x_target,float _y_target,float _r_target)
{
	x_target = _x_target;
	y_target = _y_target;
	r_target = _r_target;
}

// 正运动学解算
void Mechanum::MC_SpeedCalc()
{
    wheelTarget[RF] = -(y_target - x_target + r_target);
    wheelTarget[LF] = y_target + x_target - r_target;
    wheelTarget[LB] = y_target - x_target - r_target;
    wheelTarget[RB] = -(y_target + x_target + r_target);
}

// 逆运动学
void Mechanum::MC_ReverseCalc(int16_t *_motor_speed)
{
    float Speed_X[2] = {0.0f, 0.0f};
    float Speed_Y[2] = {0.0f, 0.0f};
    float Speed_R[2] = {0.0f, 0.0f};
    Speed_X[0] = (_motor_speed[LF] - _motor_speed[LB]) / 2.0f;
    Speed_X[1] = (_motor_speed[RF] - _motor_speed[RB]) / 2.0f;

    Speed_Y[0] = (_motor_speed[LF] - _motor_speed[RF]) / 2.0f;
    Speed_Y[1] = (_motor_speed[LB] - _motor_speed[RB]) / 2.0f;

    Speed_R[0] = -(_motor_speed[RB] + _motor_speed[LF]) / 2.0f;
    Speed_R[1] = -(_motor_speed[RF] + _motor_speed[LB]) / 2.0f;

    moveSpeed[X] = (Speed_X[0] + Speed_X[1]) / 2.0f;
    moveSpeed[Y] = (Speed_Y[0] + Speed_Y[1]) / 2.0f;
    moveSpeed[R] = (Speed_R[0] + Speed_R[1]) / 2.0f;
}
